/*
	2008-2009 UMKC Robotics Team
	ATMega128 (CPU1) 
	
	Propellor Chip Communication File
	
	Updated: 11/8/08
	
	Created By: Jared Bayne
	E-Mail:		JaredNBayne@gmail.com
	Created On:	November 8th, 2008

	Purpose:	This file will perform all the communication
				with the Propellor chip through the serial
				port USART1


*/

#include <math.h>

double mouse_x=0;
double mouse_y=0;
int X1POS=0, Y1POS=0, X2POS=0, Y2POS=0;


void fill_coordinates(void);
void fill_coordinates_debug(void);  //displays on LCD
void map_course(int speed);


void fill_coordinates(void)
{

	int	temp=-99;
	int digitsX[6] = { 0 };	//can hold up to a 15 digit number	
	int digitsY[6] = { 0 };	//can hold up to a 15 digit number
	

	writeln(":");
	//writeln("Press 1 to begin");
	//Read_Button_Press();	//waits for any button press
	

	X1POS=0;
	Y1POS=0;
	
	X2POS=0;
	Y2POS=0;
	
	
	while (scan_usart0()!='A')
		putchar0('1');
		
	//read in X1 value
	temp=getchar0(); //throw away garbage character (prop always sends this space)
	digitsX[5] = getchar0()-48;	//get sign flag	
	for (int i=0;i<=4;i++)
	{
		temp=getchar0();  //throw away garbage character (prop always sends this space)	
		digitsX[i]=getchar0()-48; //retrieve actual character		
	}

		
	//read in Y1 value
	temp=getchar0(); //throw away garbage character (prop always sends this space)
	digitsY[5] = getchar0()-48;	//get sign flag
	for (int i=0;i<=4;i++)
	{
		temp=getchar0();  //throw away garbage character (prop always sends this space)	
		digitsY[i]=getchar0()-48; //retrieve actual character

	}


	//convert X1 digits array to an integer value
	for (int i = 0; i<=4;i++)
	{	
		X1POS+=(digitsX[i])*pow(10,4-i);
	
	}
	if (digitsX[5]==1)	//negative number
		X1POS=-X1POS;
	
			
	//convert Y1 digits array to an integer value
	for (int i = 0; i<=4;i++)
	{	
		Y1POS+=(digitsY[i])*pow(10,4-i);
		
	}
	if (digitsY[5]==1)	//negative number
		Y1POS=-Y1POS;



	//read in X2 value
	temp=getchar0(); //throw away garbage character (prop always sends this space)
	digitsX[5] = getchar0()-48;	//get sign flag	
	for (int i=0;i<=4;i++)
	{
		temp=getchar0();  //throw away garbage character (prop always sends this space)	
		digitsX[i]=getchar0()-48; //retrieve actual character		
	}

		
	//read in Y2 value
	temp=getchar0(); //throw away garbage character (prop always sends this space)
	digitsY[5] = getchar0()-48;	//get sign flag
	for (int i=0;i<=4;i++)
	{
		temp=getchar0();  //throw away garbage character (prop always sends this space)	
		digitsY[i]=getchar0()-48; //retrieve actual character

	}


	//convert X digits array to an integer value
	for (int i = 0; i<=4;i++)
	{	
		X2POS+=(digitsX[i])*pow(10,4-i);
	
	}
	if (digitsX[5]==1)	//negative number
		X2POS=-X2POS;
	
			
	//convert Y digits array to an integer value
	for (int i = 0; i<=4;i++)
	{	
		Y2POS+=(digitsY[i])*pow(10,4-i);
		
	}
	if (digitsY[5]==1)	//negative number
		Y2POS=-Y2POS;

}

void fill_coordinates_debug(void)
{
	int	temp=-99;
	int digitsX[6] = { 0 };	//can hold up to a 15 digit number	
	int digitsY[6] = { 0 };	//can hold up to a 15 digit number
	

	writeln(":");
	//writeln("Press 1 to begin");
	//Read_Button_Press();	//waits for any button press
	

//	X1POS=0;
//	Y1POS=0;
	
//	X2POS=0;
//	Y2POS=0;
	
	
	while (scan_usart0()!='A')
		putchar0('1');
	
	//read in X1 value
	temp=getchar0(); //throw away garbage character (prop always sends this space)
	digitsX[5] = getchar0()-48;	//get sign flag	
	for (int i=0;i<=4;i++)
	{
		temp=getchar0();  //throw away garbage character (prop always sends this space)	
		digitsX[i]=getchar0()-48; //retrieve actual character		
	}

		
	//read in Y1 value
	temp=getchar0(); //throw away garbage character (prop always sends this space)
	digitsY[5] = getchar0()-48;	//get sign flag
	for (int i=0;i<=4;i++)
	{
		temp=getchar0();  //throw away garbage character (prop always sends this space)	
		digitsY[i]=getchar0()-48; //retrieve actual character

	}


	//convert X1 digits array to an integer value
	for (int i = 0; i<=4;i++)
	{	
		if (digitsX[5]==0)
			X1POS+=(digitsX[i])*pow(10,4-i);
		else
			X1POS-=(digitsX[i])*pow(10,4-i);
		
	
	}
	//if (digitsX[5]==1)	//negative number
	//	X1POS=-X1POS;
	
			
	//convert Y1 digits array to an integer value
	for (int i = 0; i<=4;i++)
	{	
		if (digitsY[5]==0)
			Y1POS+=(digitsY[i])*pow(10,4-i);
		else
			Y1POS-=(digitsY[i])*pow(10,4-i);
		
	}
	//if (digitsY[5]==1)	//negative number
	//	Y1POS=-Y1POS;



	//read in X2 value
	temp=getchar0(); //throw away garbage character (prop always sends this space)
	digitsX[5] = getchar0()-48;	//get sign flag	
	for (int i=0;i<=4;i++)
	{
		temp=getchar0();  //throw away garbage character (prop always sends this space)	
		digitsX[i]=getchar0()-48; //retrieve actual character		
	}

		
	//read in Y2 value
	temp=getchar0(); //throw away garbage character (prop always sends this space)
	digitsY[5] = getchar0()-48;	//get sign flag
	for (int i=0;i<=4;i++)
	{
		temp=getchar0();  //throw away garbage character (prop always sends this space)	
		digitsY[i]=getchar0()-48; //retrieve actual character

	}


	//convert X digits array to an integer value
	for (int i = 0; i<=4;i++)
	{	
		if (digitsX[5]==0)
			X2POS+=(digitsX[i])*pow(10,4-i);
		else
			X2POS-=(digitsX[i])*pow(10,4-i);
	
	}
	//if (digitsX[5]==1)	//negative number
	//	X2POS=-X2POS;
	
			
	//convert Y digits array to an integer value
	for (int i = 0; i<=4;i++)
	{	
		if (digitsY[5]==0)
			Y2POS+=(digitsY[i])*pow(10,4-i);
		else
			Y2POS-=(digitsY[i])*pow(10,4-i);
		
	}
	//if (digitsY[5]==1)	//negative number
	//	Y2POS=-Y2POS;



	writeln("X");		
	LCDDispInt(X1POS);
    write("  ");
	LCDDispInt(X2POS);
	LCDLine2();
	write("Y");
	LCDDispInt(Y1POS);
	write("  ");
	LCDDispInt(Y2POS);


	
}




void map_course(int speed)
{
	writeln("Testing Mouse");

	int counter=0;
	
	while(1)
	{
		
		drive_with_buttons(speed);
		
		if (counter>20000)
		{
			fill_coordinates_debug();
			counter=0;
		}
		counter++;
	
	}

}

